package projects.ufam.filters;

import java.awt.GridLayout;
import java.awt.geom.Point2D;
import java.util.Random;
import java.util.Vector;

import org.jfree.chart.ChartFactory;
import org.jfree.chart.ChartPanel;
import org.jfree.chart.JFreeChart;
import org.jfree.chart.plot.PlotOrientation;
import org.jfree.chart.renderer.xy.XYDotRenderer;
import org.jfree.data.xy.XYDataset;
import org.jfree.data.xy.XYSeries;
import org.jfree.data.xy.XYSeriesCollection;
import org.jfree.ui.ApplicationFrame;

import ema.Matrix;

public class KalmanFilterDemo {

	private static final double STANDARD_DEVIATION = 2;
	private static final double MEAN = 0;
	private Vector<String> data;

	public KalmanFilterDemo(Trajectory trajectory) {
		try {
			data = new Vector<String>();
			Random randx = new Random();
			Random randy = new Random();

			final double T = 1; // (10/s)
			double x = 0, y = 0;
			double xm = 0; // randx.nextGaussian(MEAN, STANDARD_DEVIATION);
			double ym = 0; // randx.nextGaussian(MEAN, STANDARD_DEVIATION);
			double xe = 0;
			double ye = 0;
			double emm = 0;
			double eme = 0;

			KalmanFilter kalman = new KalmanFilter(4, 2, 0);
			double[][] transicao = { { 1, 0, T, 0 }, { 0, 1, 0, T },
					{ 0, 0, 1, 0 }, { 0, 0, 0, 1 } };
			double[][] control = { { T * T / 2, 0 }, { 0, T * T / 2 },
					{ T, 0 }, { 0, T } };
			kalman.setTransition_matrix(transicao);
			kalman.setControl_matrix(control);
			kalman.setProcess_noise_cov(Matrix.identity(4, 4, 1e-6));
			// kalman.setMeasurement_noise_cov( new double[][] {{0.1, 0}, {0,
			// 0.1}} );

			/*
			 * KalmanFilter kalman = new KalmanFilter(6, 2); double[][]
			 * transicao = { {1, 0, T, 0, T*T/2, 0 }, {0, 1, 0, T, 0 , T*T/2 },
			 * {0, 0, 1, 0, T , 0 }, {0, 0, 0, 1, 0 , T }, {0, 0, 0, 0, 1 , 0 },
			 * {0, 0, 0, 0, 0 , 1 } }; double[][] control = { { T*T/2, 0}, { 0,
			 * T*T/2}, { T, 0 }, { 0, T }, { 1, 0 }, { 0, 1 } };
			 * kalman.setTransition_matrix( transicao );
			 * kalman.setControl_matrix(control);
			 */

			for (int k = 0; k < trajectory.numberPoints(); k++) {
				// Estimação
				double estimado[][] = kalman.predict(new double[][] {
						{ randx.nextGaussian() }, { randy.nextGaussian() } });

				// Valor estimado para o tempo k+1
				xe = estimado[0][0];
				ye = estimado[1][0];

				// Proximo passo do tempo.

				// Deslocamento Real no tempo k.
				x = trajectory.pointAt(k).getX();
				y = trajectory.pointAt(k).getY();

				// Valor medido no tempo k
				xm = x + randx.nextGaussian();
				ym = y + randy.nextGaussian();

				// Erro da Estimação e da Medição no tempo k+1
				double em = Math.pow(Point2D.distance(x, y, xm, ym), 1);
				double ee = Math.pow(Point2D.distance(x, y, xe, ye), 1);

				// Acumulanndo o erro para calcular a media
				emm += em;
				eme += ee;

				// Correção
				kalman.correct(new double[][] { { xm }, { ym } });

				// Saída
				String saida = x + "\t" + xm + "\t" + xe + "\t" + y + "\t" + ym
						+ "\t" + ye + "\t" + em + "\t" + ee + "\n";
				data.add(saida);
			}
			System.out.println("Erro Médio da Medição: "
					+ (emm / trajectory.numberPoints()));
			System.out.println("Erro Médio da Estimação: "
					+ (eme / trajectory.numberPoints()));
		} catch (Exception ex) {
			System.out.println(ex.getMessage());
		}
	}

	public String getDataAt(int index) {
		return data.elementAt(index);
	}

	public int dataSize() {
		return data.size();
	}

	public static void main(String[] args) {
		Trajectory trajectory = new Trajectory();
		trajectory.generateTrajectory(new Point2D.Double(0, 0), 1000, 1);

		KalmanFilterDemo kalmanMUV = new KalmanFilterDemo(trajectory);
		KalmanMUVGrafico g = new KalmanMUVGrafico(kalmanMUV);
		g.setSize(800, 600);
		g.setLocationRelativeTo(null);
		g.setVisible(true);
		g.setExtendedState(javax.swing.JFrame.MAXIMIZED_BOTH);
	}
}

class KalmanMUVGrafico extends ApplicationFrame {

	private static final long serialVersionUID = 5116751992250232186L;
	private KalmanFilterDemo kalmanMUV;

	public KalmanMUVGrafico(KalmanFilterDemo kalmanMUV) {
		super("Gráfico");
		this.kalmanMUV = kalmanMUV;
		JFreeChart chartPosition = createChartPosition(createDatasetPosition());
		JFreeChart chartError = createChartError(createDatasetError());
		ChartPanel chartPanelPosition = new ChartPanel(chartPosition);
		ChartPanel chartPanelError = new ChartPanel(chartError);
		this.setLayout(new GridLayout(2, 1));
		this.getContentPane().add(chartPanelPosition);
		this.getContentPane().add(chartPanelError);
	}

	private XYDataset createDatasetPosition() {
		try {
			XYSeries real = new XYSeries("Real", false);
			XYSeries medido = new XYSeries("Medido", false);
			XYSeries estimado = new XYSeries("Estimado", false);

			String linha = null;
			for (int t = 0; t < kalmanMUV.dataSize(); t++) {
				linha = kalmanMUV.getDataAt(t);
				String[] dados = linha.split("\t");
				real.add(Double.valueOf(dados[0]), Double.valueOf(dados[3]));
				medido.add(Double.valueOf(dados[1]), Double.valueOf(dados[4]));
				estimado
						.add(Double.valueOf(dados[2]), Double.valueOf(dados[5]));
				t++;
			}
			XYSeriesCollection dataset = new XYSeriesCollection();
			dataset.addSeries(real);
			dataset.addSeries(estimado);
			dataset.addSeries(medido);
			return dataset;
		} catch (Exception e) {
			System.out.println("Erro datasetPosition");
		}
		return null;
	}

	private XYDataset createDatasetError() {
		try {
			XYSeries erroMedicao = new XYSeries("Erro da Medicao");
			XYSeries erroEstimacao = new XYSeries("Erro da Estimação");
			String linha = null;
			for (int t = 0; t < kalmanMUV.dataSize(); t++) {
				linha = kalmanMUV.getDataAt(t);
				String[] dados = linha.split("\t");
				erroMedicao.add(t, Double.valueOf(dados[6]));
				erroEstimacao.add(t, Double.valueOf(dados[7]));
				t++;
			}
			XYSeriesCollection dataset = new XYSeriesCollection();
			dataset.addSeries(erroMedicao);
			dataset.addSeries(erroEstimacao);
			return dataset;
		} catch (Exception e) {
			System.out.println("Erro datasetError");
		}
		return null;
	}

	private JFreeChart createChartPosition(final XYDataset dataset) {
		JFreeChart chart = ChartFactory.createXYLineChart("", "X", "Y",
				dataset, PlotOrientation.VERTICAL, true, true, false);
		XYDotRenderer renderer = new XYDotRenderer();
		renderer.setDotHeight(4);
		renderer.setDotWidth(4);
		// XYLineAndShapeRenderer renderer = new XYLineAndShapeRenderer();
		chart.getXYPlot().setRenderer(renderer);

		chart.setBackgroundPaint(java.awt.Color.white);
		chart.getPlot().setBackgroundPaint(java.awt.Color.white);
		chart.getXYPlot().setDomainGridlinePaint(java.awt.Color.black);
		chart.getXYPlot().setRangeGridlinePaint(java.awt.Color.black);
		return chart;
	}

	private JFreeChart createChartError(XYDataset dataset) {
		JFreeChart chart = ChartFactory.createXYLineChart("", "Tempo", "Erro",
				dataset, PlotOrientation.VERTICAL, true, true, false);
		chart.getXYPlot().getRenderer().setSeriesPaint(0, java.awt.Color.GREEN);
		chart.getXYPlot().getRenderer().setSeriesPaint(1, java.awt.Color.BLUE);
		chart.setBackgroundPaint(java.awt.Color.white);
		chart.getPlot().setBackgroundPaint(java.awt.Color.white);
		chart.getXYPlot().setDomainGridlinePaint(java.awt.Color.black);
		chart.getXYPlot().setRangeGridlinePaint(java.awt.Color.black);
		return chart;
	}

}
